package de.rj.urwalking.util;

import android.content.Context;
import android.graphics.PointF;
import android.hardware.GeomagneticField;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.location.Location;
import android.location.LocationManager;
import android.opengl.Matrix;
import android.os.Bundle;

import com.google.android.gms.common.ConnectionResult;
import com.google.android.gms.common.GooglePlayServicesClient;
import com.google.android.gms.location.LocationClient;
import com.google.android.gms.location.LocationListener;
import com.google.android.gms.location.LocationRequest;

import de.rj.urwalking.config.Constants;

public class StepDetector implements LocationListener, GooglePlayServicesClient.ConnectionCallbacks,
		GooglePlayServicesClient.OnConnectionFailedListener, SensorEventListener {

	private static final long MIN_DELAY_BETWEEN_STEPS = 400000000; // = 0.4 seconds
	private static final float DEFAULT_STEP_LENGTH = 0.8f; // meters
	private static final float MIN_UPPER_THRESHOLD = 1.0f;

	private static float upperThreshold = 2.0f;
	private static float lowerThreshold = -1.0f;

	private float[] rotationMatrix = new float[16];
	private float[] directionMatrix = new float[16];
	private float[] currentValues = new float[4];
	private float[] oldValues = new float[4];

	private boolean above = false;
	private long aboveTimestamp = 0;

	private PointF directionVector = new PointF(1, 0);

	private StepListener mListener;
	private long lastStepTime = System.currentTimeMillis() * 1000000;
	private float declination;
	private LocationRequest mLocationRequest;
	private LocationClient mLocationClient;

	private SensorManager mSensorManager;
	private Context mContext;

	// Kalman stuff
	private Location newestGPSLocation;
	private Location lastFilteredLocation;
	private static final float STEP_DETECTION_VARIANCE = 0.0225f; // meters
	private long lastGPSTimestamp = 0;
	private int latestRotVecAccuracy;

	public static interface StepListener {
		public void onStep(PointF direction, float length, float variance, long timestamp);

		public void onFilteredStep(Location location);
	}

	public StepDetector(Context context, StepListener listener) {
		mContext = context;
		mListener = listener;

		initGPS();
		initSensors();
	}

	private void initGPS() {
		GeomagneticField geoField = new GeomagneticField(Constants.REGENSBURG_LAT, Constants.REGENSBURG_LON,
				Constants.REGENSBURG_ALT, System.currentTimeMillis());
		declination = geoField.getDeclination();

		mLocationRequest = LocationRequest.create();
		mLocationRequest.setPriority(LocationRequest.PRIORITY_HIGH_ACCURACY);
		mLocationRequest.setInterval(1000);
		mLocationClient = new LocationClient(mContext, this, this);
		mLocationClient.connect();
	}

	private void initSensors() {
		mSensorManager = (SensorManager) mContext.getSystemService(Context.SENSOR_SERVICE);

		mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION),
				SensorManager.SENSOR_DELAY_GAME);
		mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR),
				SensorManager.SENSOR_DELAY_GAME);

	}

	/**
	 * Resets the location.
	 */
	public void reset() {
		lastFilteredLocation = null;
	}

	/**
	 * Called when there are new linear acceleration readings.
	 * 
	 * @param event
	 * @return the processed data: mapped to world coordinates and rotated in device direction
	 */
	private float[] onLinAccEvent(SensorEvent event) {

		System.arraycopy(currentValues, 0, oldValues, 0, 4);
		System.arraycopy(event.values, 0, currentValues, 0, 3);
		Matrix.multiplyMV(currentValues, 0, rotationMatrix, 0, currentValues, 0);
		Matrix.multiplyMV(currentValues, 0, directionMatrix, 0, currentValues, 0);

		detectStep(event.timestamp);
		return currentValues;

	}

	/**
	 * Called when there are new rotation vector readings. Populates a matrix which is used to rotate the linear
	 * acceleration values.
	 * 
	 * @param event
	 */
	private void onRotVecEvent(SensorEvent event) {

		latestRotVecAccuracy = event.accuracy;

		SensorManager.getRotationMatrixFromVector(rotationMatrix, event.values);
		Matrix.invertM(rotationMatrix, 0, rotationMatrix, 0);
		// calculate rotation around z-axis
		double angle = Math.asin(event.values[2]) * 2;
		angle -= Math.PI / 2;
		directionMatrix[0] = (float) Math.cos(angle);
		directionMatrix[1] = (float) Math.sin(angle) * -1;
		directionMatrix[4] = (float) Math.sin(angle);
		directionMatrix[5] = (float) Math.cos(angle);
		directionMatrix[10] = 1;
		directionMatrix[15] = 1;
		angle -= declination * Math.PI / 180.0f;

		calculateVector(angle);
	}

	/**
	 * Uses some simple rules to determine if a step has happened.
	 * 
	 * @param timestamp
	 */
	private void detectStep(long timestamp) {

		// Ueberschreitet oberen threshold
		if (!above && (timestamp - aboveTimestamp) > MIN_DELAY_BETWEEN_STEPS && oldValues[2] < upperThreshold
				&& currentValues[2] > upperThreshold) {
			aboveTimestamp = timestamp;
			above = true;
		}
		// unterschreitet unteren threshold
		// nur moeglich wenn vorher oberer ueberquert wurde
		if (above && oldValues[2] > lowerThreshold && currentValues[2] < lowerThreshold) {
			above = false;
			lastStepTime = timestamp;
			mListener.onStep(directionVector, DEFAULT_STEP_LENGTH, STEP_DETECTION_VARIANCE, System.currentTimeMillis());

			// / Kalman stuff begins here

			// initialize location on first step
			if (lastFilteredLocation == null) {
				lastFilteredLocation = newestGPSLocation;
			}

			Location currentEstimatedLocation = calculateNewLocationFrom(lastFilteredLocation, directionVector,
					DEFAULT_STEP_LENGTH);

			float kalmanGain = calculateKalmanGain();

			Location currentFilteredLocation = applyKalmanGain(currentEstimatedLocation, kalmanGain);

			currentFilteredLocation.setAccuracy((1 - kalmanGain)
					* (lastFilteredLocation.getAccuracy() + STEP_DETECTION_VARIANCE));

			currentFilteredLocation.setExtras(new Bundle());
			currentFilteredLocation.getExtras().putInt("rotVecAccuracy", latestRotVecAccuracy);
			mListener.onFilteredStep(currentFilteredLocation);
			lastFilteredLocation = currentFilteredLocation;

		}

		// if no step was detected for some time
		if ((timestamp - lastStepTime) > 5 * MIN_DELAY_BETWEEN_STEPS) {

			if (isMovingInWalkingSpeed()) {
				// assume user is walking
				lastStepTime = timestamp;
				decreaseThresholds();
			}
		}

	}

	/**
	 * Calculates the new location by adding the step delta (expressed as direction vector and length) to the GPS
	 * coordinates.
	 * 
	 * @param oldLocation
	 * @param direction
	 * @param length
	 * @return the new location
	 */

	private Location calculateNewLocationFrom(Location oldLocation, PointF direction, float length) {
		Location newLocation = new Location(LocationManager.GPS_PROVIDER);

		newLocation.setLatitude(oldLocation.getLatitude() + 360.0f * direction.x * length / (6371000.0f * 2 * Math.PI));
		newLocation.setLongitude(oldLocation.getLongitude() + 360.0f * direction.y * length
				/ (Math.cos(oldLocation.getLatitude() * Math.PI / 180.0f) * 6371000.0f * 2 * Math.PI));

		return newLocation;
	}

	private float calculateKalmanGain() {

		if (lastGPSTimestamp == newestGPSLocation.getTime()) {
			// Set the Kalman gain to zero (= no influence for GPS location) if
			// we already used this GPS location, i.e. there was no new GPS fix
			// since the last step.
			return 0;
		} else {
			lastGPSTimestamp = newestGPSLocation.getTime();
			return (lastFilteredLocation.getAccuracy() + STEP_DETECTION_VARIANCE)
					/ (lastFilteredLocation.getAccuracy() + STEP_DETECTION_VARIANCE + newestGPSLocation.getAccuracy());
		}

	}

	/**
	 * 
	 * @param currentEstimatedLocation
	 *            the location that we got by adding the step delta
	 * @param kalmanGain
	 *            the influence of the GPS location
	 * @return the filtered location
	 */

	private Location applyKalmanGain(Location currentEstimatedLocation, float kalmanGain) {

		double latDiff = newestGPSLocation.getLatitude() - currentEstimatedLocation.getLatitude();
		double lonDiff = newestGPSLocation.getLongitude() - currentEstimatedLocation.getLongitude();

		double newLatitude = currentEstimatedLocation.getLatitude() + kalmanGain * latDiff;
		double newLongitude = currentEstimatedLocation.getLongitude() + kalmanGain * lonDiff;

		Location currentFilteredLocation = new Location(LocationManager.GPS_PROVIDER);

		currentFilteredLocation.setLatitude(newLatitude);
		currentFilteredLocation.setLongitude(newLongitude);

		return currentFilteredLocation;
	}

	/**
	 * Uses the latest GPS location to determine if the user moves in a specific speed interval. //TODO: use floating
	 * average
	 * 
	 */

	private boolean isMovingInWalkingSpeed() {

		Location location = newestGPSLocation;

		if (location == null) return false;

		final float minWalkingSpeed = 1.0f; // = 3.6 km/h
		final float maxWalkingSpeed = 2.0f; // = 7.2 km/h

		if (location.getSpeed() > minWalkingSpeed && location.getSpeed() < maxWalkingSpeed) {
			return true;
		} else {
			return false;
		}

	}

	/**
	 * Decreases the upper and lower thresholds used for step detection, unless they are already below a certain limit.
	 */

	private void decreaseThresholds() {

		if (upperThreshold > MIN_UPPER_THRESHOLD) {
			lowerThreshold *= 0.9;
			upperThreshold *= 0.9;
		}
	}

	private void calculateVector(double angle) {
		directionVector.set((float) Math.cos(angle), -1 * (float) Math.sin(angle));
	}

	public float getLowerThreshold() {
		return lowerThreshold;
	}

	public float getUpperThreshold() {
		return upperThreshold;
	}

	@Override
	public void onConnectionFailed(ConnectionResult arg0) {
		// TODO Auto-generated method stub

	}

	@Override
	public void onConnected(Bundle arg0) {
		mLocationClient.requestLocationUpdates(mLocationRequest, this);

	}

	@Override
	public void onDisconnected() {
		// TODO Auto-generated method stub

	}

	@Override
	public void onLocationChanged(Location location) {
		// convert accuracy from standard deviation to variance
		float variance = location.getAccuracy() * location.getAccuracy();
		location.setAccuracy(variance);
		newestGPSLocation = location;

	}

	@Override
	public void onSensorChanged(SensorEvent event) {

		if (event.sensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION) {
			onLinAccEvent(event);
		} else if (event.sensor.getType() == Sensor.TYPE_ROTATION_VECTOR) {
			onRotVecEvent(event);
		}
	}

	@Override
	public void onAccuracyChanged(Sensor sensor, int accuracy) {
		// TODO Auto-generated method stub

	}

	public void stop() {

		mSensorManager.unregisterListener(this);
		if (mLocationClient.isConnected()) {			
			mLocationClient.removeLocationUpdates(this);
		}
		mLocationClient.disconnect();
	}

}
